/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: EmergencyStopServer.cpp,v 1.12 2009-11-27 13:34:51 morsko Exp $
 *
 * \file
 * \brief Implementation of EmergencyStop interface Server.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */

#include "EmergencyStopServer.hpp"
#include "binbag.h"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::EmergencyStop;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CEmergencyStopServer::CEmergencyStopServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, 
                                 const int aInterfaceMinor,
                                 const std::string &aDescription)
  : CMaCI(aMaCICtrlServerPtr, GIMI_PROTOCOL_MACI_EMERGENCYSTOP, aInterfaceMinor),
    CSync(1,1),
    CThread(2),
    iServerActive(false),
    iServiceDescription(aDescription),
    iTimeoutTime_ms(500),
    iClientDropTime(),
    iTimeoutTimesSet(false),
    iEmergencyStopArray(),
    iMessageSourceArray(),
    iActiveClients()
{
  //  SetInterfaceSubscriberSyncLimits(5, 1000);
}
//*****************************************************************************

CEmergencyStopServer::~CEmergencyStopServer()
{
  DoClose();
}
//*****************************************************************************

MaCI::EMaCIError CEmergencyStopServer::DoOpen(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);
  
  // Execute only if server is not active.
  if (!iServerActive && iTimeoutTimesSet) {
    // Register service.
    RegisterProvidedService("MaCI-EmergencyStopServer-EmergencyStop; '" + iServiceDescription + "'");
    RegisterAcceptedService("MaCI-EmergencyStopServer-Cmd; '" + iServiceDescription + "'");

    iServerActive = true;
    SetActive(true);
    CThread::RunThread(0);
    CThread::RunThread(1);
    result = KMaCIOK;
    
    dPrint(1,"Succesfully Open():ed EmergencyStopServer instance %p.", this);
  }else{
    if(iServerActive){
      dPrint(3,"Server already opened");
    }

    if(iTimeoutTimesSet){
      dPrint(1,"Timeout time hasn't been set. Please set them before opening!");
    }
  }
    
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CEmergencyStopServer::DoClose(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);

  // Execute block only if server is currently active.
  if (iServerActive) {
    SetActive(false);
    // Stop the handler Thread
    //   /* CThread::CancelThread(); //Removed by script */
    CThread::WaitThread(0);    
    CThread::WaitThread(1);    
    // Unregister services
    UnregisterProvidedService();
    UnregisterAcceptedService();
    
    // Stop the handler Thread
    iServerActive = false;
    
    dPrint(4,"Succesfully Close():ed EmergencyStopServer instance %p", this);
  }
  
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CEmergencyStopServer::WaitCommand(CEmergencyStopData &data, 
                                                   SMessageSource &aSource,
                                                   const unsigned int aTimeout_ms,
                                                   bool aSkipQueued)
{
  EMaCIError result = KMaCIError;
  const unsigned int begin = ownTime_get_ms();
  int timeout = aTimeout_ms;

  data.Reset();

  Lock();
  do {
    const unsigned int asize = iEmergencyStopArray.size();
    // Check whether there are commands in buffer.
    if (asize > 0) {
      // Got element. So; take reference of its pointer, copy it to
      // 'data', delete it. Pop the element from queue.
      CEmergencyStopData *d;
      SMessageSource *s;
      // Check whether we want to skip queue 
      if (aSkipQueued && asize > 1) {
        // This is only executed if there is >1 in queue.  Otherwise
        // default branch is executed.  Want to skip, so get back()
        // reference.
        d = iEmergencyStopArray.back();
        s = iMessageSourceArray.back();
        // Copy data
        data = *d;
        aSource = *s;
        // Delete all elements.
        for(EACH_IN_i(iEmergencyStopArray)) {
          delete *i;
        }
        for(EACH_IN_i(iMessageSourceArray)) {
          delete *i;
        }
        // Clear array.
        iEmergencyStopArray.clear();
        iMessageSourceArray.clear();
        // print debug
        dPrint(4,"Flushed %d unprocessed commands out of buffer",
               asize-1);

      } else {
        // No skipping, so get front() reference;
        d = iEmergencyStopArray.front();
        s = iMessageSourceArray.front();
        // Copy data.
        data = *d;
        aSource = *s;
        // Delete source
        delete d;
        delete s;
        // pop the entry from queue.
        iEmergencyStopArray.pop_front();
        iMessageSourceArray.pop_front();

      }

      // Set result.
      result = KMaCIOK;
      
      // Break the loop, skipping Wait()
      break;
    }
    
    // Wait signal unconditionally.
    if (timeout > 0) Wait(timeout);
    
    // Recalculate timeout
    timeout = aTimeout_ms - ownTime_get_ms_since(begin);
  } while(timeout > 0 && IsActive());
  Unlock();

  // Change result type if timeout was the reason (Depends that
  // timeout is zero or equal, and the result was not previously set
  // to OK)
  if (timeout <= 0 && result == KMaCIError) result = KMaCITimeout;


  return result;
}
//*****************************************************************************

bool CEmergencyStopServer::SendEmergencyStopEvent(const CEmergencyStopData &aData)
{
  bool result = false;

  assert(iGIMIPtr != NULL);
  //aData.Print(1);
  const TStatus *s = aData.GetStatus();
  if (s){
    EMaCIError e = 
      SendInterfaceSubscriberMessage(aData.GetBinBagContainerPtr()->GetBuffer(),
                                     aData.GetBinBagContainerPtr()->GetBufferSize());
    result = (e == KMaCIOK);

  } else {
    dPrint(4,"No TStatus element in container, will not send!");

  }

  return result;
}
//*****************************************************************************

int CEmergencyStopServer::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CEmergencyStopData d;
  bool r;

  // Before even attempting to decode, read access.
  MaCICtrl::TAccess a;
  CMaCI::GetMaCICtrlServer()->GetAccessInfo(a, 
                                            iInterfaceMajor,
                                            iInterfaceMinor,
                                            msg.getSenderId(),
                                            true);
  // Test for access.

  if (a.IsAccess()) {
    dPrint(8,"Access granted for nodeID: %08X, proceed with command.",
           msg.getSenderId());
    
    r = bb.Decode(msg.getData(), msg.getDatalength());
    if (r) {
  
      // Succesfully decoded as BinBag.
      r = d.DecodeFrom(&bb);
      if (r) {
        // Succesfully decoded as EmergencyStop
        // Now, Check whether it was Command, if so, process it and construct reply
        gim::binbag::CBinBag replybb;
        CEmergencyStopData reply;
        bool send_reply = false;

        // Do encoding to this binbag
        reply.EncodeTo(&replybb);
    
        const TCommand *cmd = d.GetCommand();
        if (cmd) {
          switch(cmd->cmd) {
          case KCommandClientAlive:{
            std::map<int,MaCI::Common::TTimestamp>::iterator iter = iActiveClients.find(msg.getSenderId()) ;
            if(iter != iActiveClients.end()){
              iter->second.SetToCurrentTime();

            }
              break;
            }
          
          case KCommandGetTimeoutTime:
          case KCommandActivateClient:{

              reply.SetCommand(KCommandGetTimeoutReply);
              reply.SetTimeoutTime(TTimeoutTime(iTimeoutTime_ms));
              Lock();
              //if client id is not already added to map, then add it!
              if(iActiveClients.find(msg.getSenderId()) == iActiveClients.end()){
                dPrint(4,"Adding client '%p' !",msg.getSenderId());
                MaCI::Common::TTimestamp tstamp;
                tstamp.SetToCurrentTime();

                iActiveClients.insert(std::make_pair(msg.getSenderId(),tstamp));

                
              }else{
                dPrint(5,"Client alredy activated '%p'",msg.getSenderId());
              }
              Unlock();
              send_reply = true;
              break;
            } 
          case KCommandActivateEmergencyStop:
          case KCommandDeactivateEmergencyStop:{
            // Create new dynamic entry

            CEmergencyStopData *new_entry = new CEmergencyStopData(d);
            SMessageSource *source = new SMessageSource(msg);
            Lock();
            iEmergencyStopArray.push_back(new_entry);
            iMessageSourceArray.push_back(source);
            Notify();
            Unlock();
            break;

          }
            

           
          case KCommandDeactivateClient:{

            Lock();
            std::map<int,MaCI::Common::TTimestamp>::iterator iter = iActiveClients.find(msg.getSenderId()) ;
            if(iter != iActiveClients.end()){
              dPrint(6,"Removing client '%p'",iter->first);
              iActiveClients.erase(iter);
            }else{
              dPrint(8,"Client '%p' not found from active client list. So, ignoring message :)",msg.getSenderId());
            }

            Unlock();
            reply.SetCommand(KCommandDeactivateClient);
            send_reply = true;
              break;
            }
          default:
            dPrint(3,"Unidentified command! %d (%x)! Ignore.",
                   cmd->cmd, cmd->cmd);
            break;
          }
        }
      
        // Check whether reply needs to be sent
        if (send_reply) {
          dPrint(10,"Sending reply TO 0x%x , with messageID %d", msg.getSenderId(),   msg.messageId);
          reply.Print(10);
          MaCI::EMaCIError e = SendSynchronousMessageReply( replybb.GetBuffer(),
                                                            replybb.GetBufferSize(),
                                                            msg.getSenderId(),
                                                            iInterfaceMajor,
                                                            iInterfaceMinor,
                                                            msg.messageId);
                                                            
          //          if (gr == GIMI_OK) {
          if(e == KMaCIOK){
            dPrint(8,"Succesfully sent message with %d BinBag elements to '%p'",
                   replybb.GetElementCount(),
                   msg.getSenderId());

          } else {
	  
            dPrint(8,"Failed to send message with %d BinBag elements to '%p'",
                   replybb.GetElementCount(),
                   msg.getSenderId());
          }
        }
      }
    }
  } else {
    dPrint(2,"Access denied for nodeID: %08X, will not execute command.",
           msg.getSenderId());

  }  
  return result;
}
//*****************************************************************************

int CEmergencyStopServer::ThreadFunction(const int aThreadNumber)
{

  assert(iGIMIPtr != NULL);

  switch(aThreadNumber){
  case 0:{
    return IncomingMessageHandlerThread();
  }
  case 1:{
    return ClientMonitoringThread();
  }
  }
  return -1;
}
//*****************************************************************************

int CEmergencyStopServer::IncomingMessageHandlerThread(){

  int result = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
    
  dPrint(4, "GIMIMessage handler started");
    
  // Run as long as the thread should be active
  while(IsActive()) {
    e = ReceiveInterfaceMessage(gmsg,1000);
    
    if(e == KMaCIOK){
      
      ProcessMessage(gmsg);
      
    } else if (e != KMaCITimeout) {
      // Other error, try to recover by sleeping one second.
      dPrint(4,"Received error from ReceiveInterfaceMessage! Abort!");
      break;
    }
  }
  
  dPrint(4, "GIMIMessage handler stopped");
  SetActive(false);
  
  return result;
}
//*****************************************************************************

int CEmergencyStopServer::ClientMonitoringThread(){

    std::map<int,MaCI::Common::TTimestamp>::iterator iter;
    MaCI::Common::TTimestamp timenow;

    while(IsActive()){
      if(iActiveClients.size() >0){

        timenow.SetToCurrentTime();
        std::list<std::map<int,MaCI::Common::TTimestamp>::iterator> clientsToRemove;
        Lock();
        
        for(iter = iActiveClients.begin(); iter != iActiveClients.end(); iter++){
          
          
          if(timenow.GetGimTime()-iter->second.GetGimTime() > iTimeoutTime_ms){
            dPrintLC(1,OWNDEBUG_COLOR_ATTR_BRIGHT,OWNDEBUG_COLOR_RED, OWNDEBUG_COLOR_BLACK, "Emergency: Client '%p' has not sent an activation message within timeout time(last message '%s' ago). Generating an Emergency stop message!)",
                     iter->first,
                     (timenow.GetGimTime()-iter->second.GetGimTime()).toString().c_str());
            CEmergencyStopData *d = new CEmergencyStopData();
            d->CreateInternalBinBag();
            d->SetCommand(TCommand(KCommandActivateEmergencyStop));
            SMessageSource *source = new SMessageSource(0,GIMI_ID_MIN,0,0);
            
            iEmergencyStopArray.push_back(d);
            iMessageSourceArray.push_back(source);
            Notify();
          
          }
          if(timenow.GetGimTime()-iter->second.GetGimTime() > iClientDropTime.GetGimTime()){
            dPrintLC(1,OWNDEBUG_COLOR_ATTR_BRIGHT,OWNDEBUG_COLOR_RED, OWNDEBUG_COLOR_BLACK, "Emergency: Client '%p' has not sent an activation message within '%s'. Removing it from active client list!)",     
                     iter->first,
                     iClientDropTime.GetGimTime().toString().c_str());

            clientsToRemove.push_back(iter);
            
          }
        }
        //Remove clients...
        while(clientsToRemove.size() != 0){
          iActiveClients.erase(clientsToRemove.front());
          clientsToRemove.pop_front();
        }
        
        Unlock();
      }else{
        CEmergencyStopData *d = new CEmergencyStopData();
        d->CreateInternalBinBag();
        d->SetCommand(TCommand(KCommandNoActiveClients));
        SMessageSource *source = new SMessageSource(0,GIMI_ID_MIN,0,0);
        
        iEmergencyStopArray.push_back(d);
        iMessageSourceArray.push_back(source);
        Notify();
        dPrint(10,"No active clients.. added a message to the queue for the module");
      }
      ownSleep_ms(iTimeoutTime_ms);
    }
    return 0;


}
//*****************************************************************************

MaCI::EMaCIError CEmergencyStopServer::SendReply(const CEmergencyStopData &aData,
                                                 const SMessageSource &aSource)
{
  EMaCIError result = KMaCIError;
  // Check that server is active.
  //Second check is for not replying when server autogenerates emergencystops when client is not reachable.
  if (iServerActive && aSource.majorNumber == GIMI_PROTOCOL_MACI_EMERGENCYSTOP) {
    // Send

    aSource.Print(10);
    aData.Print(10);
    result = SendSynchronousMessageReply(aData.GetBinBagContainerPtr()->GetBuffer(),
                                         aData.GetBinBagContainerPtr()->GetBufferSize(),
                                         aSource);
    
  } else {
    if(!iServerActive){
      dPrint(2,"Unable to send ParameterData - Interface is not active! (Open() not called?)");
      result = KMaCINotActive;
    }else{
      result = KMaCIOK;
    }
  }

  return result;
}
//*****************************************************************************

void CEmergencyStopServer::SetTimeoutTime(unsigned int aTimeout_ms,
                                          MaCI::Common::TTimestamp aClientDropTime)
{
  Lock();
  iTimeoutTime_ms = aTimeout_ms;
  iClientDropTime = aClientDropTime;
  iTimeoutTimesSet = true;
  Unlock();

}
//*****************************************************************************

